
#include <stdio.h>
#include <BHingeRestraint>
#include "member_BHingeRestraint.h"
#include "member_BRestraint.h"
#include "member_BBody.h"
#include "member_BNode.h"

using namespace BWE;

#define member					(*(member_BHingeRestraint*)_ptr)
#define member_allocate()		_ptr = new member_BHingeRestraint(this)
#define member_release()		delete (member_BHingeRestraint*)_ptr

BHingeRestraint::BHingeRestraint()
{
	member_allocate();
}
BHingeRestraint::BHingeRestraint(const BString& name)
{
	member_allocate();
}
BHingeRestraint::BHingeRestraint(BBody* bodyA, const BVector& pivotA) : BRestraint(bodyA, pivotA)
{
	member_allocate();
}
BHingeRestraint::BHingeRestraint(BBody* bodyA, BBody* bodyB) : BRestraint(bodyA, bodyB)
{
	member_allocate();
}
BHingeRestraint::BHingeRestraint(BBody* bodyA, const BVector& pivotA, BBody* bodyB, const BVector& pivotB) : BRestraint(bodyA, pivotA, bodyB, pivotB)
{
	member_allocate();
}
BHingeRestraint::~BHingeRestraint()
{
	member_release();
}

void BHingeRestraint::copy(const BRestraint* other)
{
	BRestraint::copy(other);

	member.axisA = hinge_member(other)->axisA;
	member.axisB = hinge_member(other)->axisB;

	member.angularLimit = hinge_member(other)->angularLimit;

	member.motorEnabled = hinge_member(other)->motorEnabled;
	member.motorTarget = hinge_member(other)->motorTarget;
	member.motorVelocity = hinge_member(other)->motorVelocity;
	member.motorMaxForce = hinge_member(other)->motorMaxForce;
}
BRestraint* BHingeRestraint::clone() const
{
	BHingeRestraint* restraint = new BHingeRestraint();
	restraint->copy(this);
	return restraint;
}

bool BHingeRestraint::setAxisA(BReal ax, BReal ay, BReal az)
{
	return setAxisA(BVector(ax, ay, az));
}
bool BHingeRestraint::setAxisA(const BVector& axisA)
{
	if (member.axisA != axisA)
	{
		member.axisA = axisA;
		this->emit(Signal_Changed);
		return true;
	}
	return false;
}
const BVector& BHingeRestraint::axisA() const
{
	return member.axisA;
}

bool BHingeRestraint::setAxisB(BReal ax, BReal ay, BReal az)
{
	return setAxisB(BVector(ax, ay, az));
}
bool BHingeRestraint::setAxisB(const BVector& axisB)
{
	if (member.axisB != axisB)
	{
		member.axisB = axisB;
		this->emit(Signal_Changed);
		return true;
	}
	return false;
}
const BVector& BHingeRestraint::axisB() const
{
	return member.axisB;
}

void BHingeRestraint::setAngularLimit(const BRealRange& angularLimit)
{
	member.angularLimit = angularLimit;
}
const BRealRange& BHingeRestraint::angularLimit() const
{
	return member.angularLimit;
}

void BHingeRestraint::setMotorEnabled(bool motorEnabled)
{
	if (member.motorEnabled != motorEnabled)
	{
		member.motorEnabled = motorEnabled;
		if (member.motorReached)
		{
			member.motorReached = false;
			this->emit(Signal_AngularMotorReached, false);
		}
		this->emit(Signal_AngularMotorEnabled, motorEnabled);
	}
}
bool BHingeRestraint::motorEnabled() const
{
	return member.motorEnabled;
}

void BHingeRestraint::setMotorTarget(BReal motorTarget)
{
	if (member.motorTarget != motorTarget)
	{
		member.motorTarget = motorTarget;
		if (member.motorReached)
		{
			member.motorReached = false;
			this->emit(Signal_AngularMotorReached, false);
		}
		this->emit(Signal_AngularMotorChanged);
	}
}
BReal BHingeRestraint::motorTarget() const
{
	return member.motorTarget;
}

void BHingeRestraint::setMotorVelocity(BReal motorVelocity)
{
	if (member.motorVelocity != motorVelocity)
	{
		member.motorVelocity = motorVelocity;
		this->emit(Signal_AngularMotorChanged);
	}
}
BReal BHingeRestraint::motorVelocity() const
{
	return member.motorVelocity;
}

void BHingeRestraint::setMotorMaxForce(BReal motorMaxForce)
{
	if (member.motorMaxForce != motorMaxForce)
	{
		member.motorMaxForce = motorMaxForce;
		this->emit(Signal_AngularMotorChanged);
	}
}
BReal BHingeRestraint::motorMaxForce() const
{
	return member.motorMaxForce;
}

const BVector& BHingeRestraint::horAxis() const
{
	return member.horAxis;
}
const BVector& BHingeRestraint::verAxis() const
{
	return member.verAxis;
}
const BVector& BHingeRestraint::dirAxis() const
{
	return member.dirAxis;
}
const BVector& BHingeRestraint::swingAxis() const
{
	return member.swingAxis;
}

BReal BHingeRestraint::hingeAngle() const
{
	return member.hingeAngle;
}
bool BHingeRestraint::motorReached() const
{
	return member.motorReached;
}

int BHingeRestraint::solverSize()
{
	member.caculate();
	if (member.solveLimit || member.motorEnabled)
	{
		return 6;
	}
	return 5;
}
void BHingeRestraint::soverItems(BSolverItem* solverItems, BReal stepTime)
{
	const BBody* bodyA = restraint_member(this)->bodyA;
	const BBody* bodyB = restraint_member(this)->bodyB;

	const BMatrix& matrixA = node_member(bodyA)->matrix;
	const BMatrix& matrixB = node_member(bodyB)->matrix;
	const BVector& angularVelocityA = body_member(bodyA)->angularVelocity;
	const BVector& angularVelocityB = body_member(bodyB)->angularVelocity;

	BVector pointA = matrixA * restraint_member(this)->pivotA;
	BVector pointB = matrixB * restraint_member(this)->pivotB;
	BVector offset = pointB - pointA;

	BReal invMassA = solverItems[0].invMassA;
	BReal invMassB = solverItems[0].invMassA;
	BReal invMassSum = invMassA + invMassB;
	BReal factA = BReal(0.5);
	if (invMassSum > 0)
	{
		factA = invMassB / invMassSum;
	}
	BReal factB = BReal(1) - factA;

	BVector axisA = matrixA.mult3(member.axisA.normal());
	BVector axisB = matrixB.mult3(member.axisB.normal());
	BVector axis = axisA * factA + axisB * factB;
	if (axis.length2() < REAL_EPSILON)
	{
		factA = 0;
		factB = 1;
		axis = axisA * factA + axisB * factB;
	}
	axis.normalize();

	BVector relB = pointB - matrixB.position();
	BVector projB = axis * relB.dot(axis);
	BVector orthoB = relB - projB;

	BVector relA = pointA - matrixA.position();
	BVector projA = axis * relA.dot(axis);
	BVector orthoA = relA - projA;

	BVector totalDist = projA - projB;
	relA = orthoA + totalDist * factA;
	relB = orthoB - totalDist * factB;
	BVector p = orthoB * factA + orthoA * factB;
	BReal len2 = p.length2();
	if (len2 > REAL_EPSILON)
	{
		p /= sqrt(len2);
	}
	else
	{
		p = matrixA.row(0);
	}
	BVector q = axis.cross(p);

	solverItems[0].torqueAxisA = relA.cross(p);
	solverItems[0].torqueAxisB = -relB.cross(p);
	solverItems[0].contactNormalA = p;
	solverItems[0].contactNormalB = -p;
	solverItems[0].velocity = p.dot(offset) / stepTime;

	bool hasStaticBody = (invMassA < REAL_EPSILON) || (invMassB < REAL_EPSILON);
	solverItems[1].torqueAxisA = relA.cross(q);
	solverItems[1].torqueAxisB = -relB.cross(q);
	if (hasStaticBody && member.solveLimit)
	{
		solverItems[1].torqueAxisA *= factA;
		solverItems[1].torqueAxisB *= factB;
	}
	solverItems[1].contactNormalA = q;
	solverItems[1].contactNormalB = -q;
	solverItems[1].velocity = q.dot(offset) / stepTime;

	solverItems[2].torqueAxisA = relA.cross(axis);
	solverItems[2].torqueAxisB = -relB.cross(axis);
	if (hasStaticBody)
	{
		solverItems[2].torqueAxisA *= factA;
		solverItems[2].torqueAxisB *= factB;
	}
	solverItems[2].contactNormalA = axis;
	solverItems[2].contactNormalB = -axis;
	solverItems[2].velocity = axis.dot(offset) / stepTime;

	BVector u = axisA.cross(axisB);

	solverItems[3].torqueAxisA = p;
	solverItems[3].torqueAxisB = -p;
	solverItems[3].velocity = u.dot(p) / stepTime;

	solverItems[4].torqueAxisA = q;
	solverItems[4].torqueAxisB = -q;
	solverItems[4].velocity = u.dot(q) / stepTime;

	if (member.solveLimit || member.motorEnabled)
	{
		solverItems[5].torqueAxisA = -axis;
		solverItems[5].torqueAxisB = axis;

		solverItems[5].velocity = 0;
		if (member.motorEnabled && !bZero(member.motorVelocity))
		{
			BReal factor = member.motorFactor(stepTime);
			solverItems[5].velocity += factor * member.motorVelocity;
			solverItems[5].impulseLowerLimit = -member.motorMaxForce * stepTime;
			solverItems[5].impulseUpperLimit = member.motorMaxForce * stepTime;
		}
		if (member.solveLimit)
		{
			solverItems[5].velocity += member.angleError / stepTime * 0.5;
			BReal limit = member.angularLimit.size();
			if (bZero(limit))
			{
				solverItems[5].impulseLowerLimit = -REAL_MAX;
				solverItems[5].impulseUpperLimit = REAL_MAX;
			}
			else if (member.angleError > 0)
			{
				solverItems[5].impulseLowerLimit = 0;
				solverItems[5].impulseUpperLimit = REAL_MAX;
			}
			else if (member.angleError < 0)
			{
				solverItems[5].impulseLowerLimit = -REAL_MAX;
				solverItems[5].impulseUpperLimit = 0;
			}
		}
	}
}
